#ifndef UTILS_H
#define UTILS_H

// 系统头文件



#include <float.h>
#include <iostream>
#include <map>
#include <math.h>
#include <queue>
#include <string>
#include <unordered_map>
#include <vector>
#include <stdint.h>
#include <inttypes.h>
#include <cstdlib>
#include <fstream>
#include <unistd.h>
#include <time.h>
#include <stdio.h>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/SVD>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>
#include <signal.h>
#include <utility>

// 消息头文件
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/path.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/float64.hpp>
#include <std_msgs/msg/float32.hpp>
#include <std_msgs/msg/string.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

// 自定义头文件
#include "common/TrajectoryTracking.h"
#include "common/TrajectoryTrackingTruck.h"
#include "common/TrajectoryTracking_mag.h"
#include "common/pid_control.h"

#include <yaml-cpp/yaml.h>

namespace control
{

//
const uint8_t ESTOP        = 1;
const uint8_t BRAKE        = 2;
// math
const double EPSILON       = 1e-4;
const double SPEED_EPSILON = 1e-2;
const double PI            = 3.14159265358979323846;
const double MAX_NUM       = 100000000000000000000.0;

const double MAX_TRACE_NUM = 800.0; // 每个文件最大数据流，由于程序执行的周期为10HZ，取值800代表8s生成一个文件
const double MAX_FILE_NUM  = 450.0;  // 能保存的最多文件个数，450*8s=3600s=1h，即保存最近一个小时的数据

/*将角度任意圆整到：0~+2*PI*/
static double wrapTo2PI(double lambda)
{
  double b;
  b = fmod(lambda, 2.0 * PI);
  if (b < 0)
  {
    b = b + 2.0 * PI;
  }
  return b;
}

/*将角度任意圆整到：-PI~+PI*/
static double wrapToPI(double lambda)
{
  double b = lambda;
  if (lambda > PI || lambda < -PI)
  {
    b = wrapTo2PI(lambda + PI) - PI;
  }
  return b;
}

// 将角度变为弧度
static double angle2Radian(double angle)
{
  return angle * PI / 180;
}

// 将弧度变为角度
static double radian2Angle(double radian)
{
  return radian * 180 / PI;
}

}

#endif // UTILS_H
